/*
 * Team D VEX auxiliary autonomous code.
 * @author Derek Leung, Michael Searing
 */

 
//////////////////////////
/////// Auto Drive ///////
//////////////////////////
//Autonomous drive function
void drive(int direction, int distance, int power, int turn) //-1 or 1,inches,max motor power,-1 to 1
{
	SensorValue(lshaft) = SensorValue(rshaft) = 0;
	drive_target = 360 * 1/(PI * 2.8) * distance;
	lturn = 1 + 2*turn;
	if(turn > 0)
	  lturn = 1;
	rturn = 1 - 2*turn;
	if(turn < 0)
	  rturn = -1;
  drive_errprev=0;
	while(drive_target - (abs(SensorValue(lshaft)) + abs(SensorValue(rshaft)))/2 > 20)
	{
		left = SensorValue(lshaft);
		right = SensorValue(rshaft);
		drive_distance = (abs(left) + abs(right))/2;
		drive_err = drive_target - drive_distance;
		if(abs(drive_err) > 200)
	  	correction = 4*(abs(left) - abs(right));
	  else
	    correction = 0;
	  pturn = 1 + abs(turn);
    drive_ppower = 0.27*drive_err*pturn;
    drive_dpower = 2.5*(drive_err - drive_errprev);
    drive_power = drive_ppower + drive_dpower;
    if(drive_power > power)
      drive_power = power;
    if(drive_power < -power)
      drive_power = -power;
    if(!debug)
    {
	    motor[leftDB] = -direction * (drive_power - correction) * lturn;
	    motor[rightDB] = -direction * (drive_power + correction) * rturn;
	  }
    drive_errprev = drive_err;
    wait1Msec(15);
	}
}

////////////////////////
/////// Auto Arm ///////
////////////////////////
//Ultrasonic range checker
bool ultra_range()
{
  if(SensorValue(ultra) <= ULTRA_MAX)
    return true;
  else
    return false;
}

//Intake control
void regulate_intake(int intake_direction)
{
  motor[port7] = 127*intake_direction;
}

//Auto arm function
void regulate_arm(int arm_target)
{
  arm_errtot = 0;
  while(arm_move && (vexRT[Btn7UXmtr2] == 1 || vexRT[Btn7LXmtr2] == 1 || vexRT[Btn7RXmtr2] == 1 || vexRT[Btn7DXmtr2] == 1 || auton))
  {
    if(abs(SensorValue(pot) - arm_target) > ARM_MAXERR)
    {
      if(abs(real_pot - SensorValue(pot)) > 1) //Prevents pot fluctuations from causing unwanted dpower
        real_pot = SensorValue(pot);
      arm_err = arm_target - real_pot;
      if(abs(arm_err) < (real_pot/5 - 60)) //Enable I when close to target
        arm_errtot = arm_errtot + arm_err/15.0;
      else
        arm_errtot = 0;
      if(abs(arm_errtot) > ARM_IMAX) //Reset I
        arm_errtot = 0;
      arm_ppower = ARM_PMULT*arm_err;
      arm_ipower = ARM_IMULT*arm_errtot;
      arm_dpower = ARM_DMULT*(arm_err - arm_errprev);
      arm_power = -(arm_ppower + arm_ipower + arm_dpower);
      if(!debug)
      {
        motor[port1] = -arm_power;
        motor[port3] = arm_power;
        motor[port8] = -arm_power;
        motor[port10]= arm_power;
      }
      arm_errprev = arm_err;
      wait1Msec(15);
    }
    else
      arm_move = false;
  }
}

//Intaking operator autonomous function
void opauto()
{
  arm_move = true;
  regulate_arm(ARM_6);
  if(ultra_range())
  {
    intake_move = true;
    regulate_intake(1);
    arm_move = true;
    regulate_arm(ARM_BOTTOM);
    intake_move = false;
  }
}

//////////////////////////////
/////// Operator Input ///////
//////////////////////////////
//Checking for operator button presses
task arm_control()
{
  while(true)
  {
    if(vexRT[Btn7UXmtr2] == 1)
    {
      arm_move = true;
     	regulate_arm(ARM_30);
    }
    else if(vexRT[Btn7LXmtr2] == 1)
    {
      arm_move = true;
    	regulate_arm(ARM_20);
    }
    else if(vexRT[Btn7RXmtr2] == 1)
    {
      arm_move = true;
	    regulate_arm(ARM_10);
    }
    else if(vexRT[Btn7DXmtr2] == 1 || auton)
    {
      arm_move = true;
      opauto();
    }
    else
    {
      arm_move = false;
      intake_move = false;
    }
  }
}

// DRIVE BASE //
/** Input is target. */
/*int drivebaseAuto(int target, bool useUltra) { // TODO not completed
  if(use_ultra)
    dist_covered = ultra_start - SensorValue(ultra);
  else
    dist_covered = SensorValue(lshaft) - drive_base_offset_left; //uses left drive base error
  if(drive_base_alert != -1) {
    if(dist_covered > drive_base_alert) {
      drive_base_alert = -1;
    }
  }
  if(dist_covered >= drive_base_target_linear) { // TODO include angular
    TODO refine motor kill
    motor[leftDBMotor] = 0;
    motor[rightDBMotor]= 0;
    drive_base_target_reached = true;
    drive_base_move = false; // TODO does not include angular movement
  } else {
    motor[leftDBMotor] = P_DB * (dist_covered - drive_base_target_linear);
    motor[rightDBMotor]= P_DB * (dist_covered - drive_base_target_linear);
  }
    // // return 0;
}*/